/*
 * Elevator.cpp
 *
 *  Created on: Mar 7, 2012
 *      Author: ajeffery
 */

#include "Elevator.h"
#include <stdint.h>
#include <native/mutex.h>
#include <rtdk.h>

//Mutexes
RT_MUTEX directionMutex;
RT_MUTEX curFloorMutex;
RT_MUTEX hallCallsMutex;
RT_MUTEX internalCallsMutex;
RT_MUTEX destFloorMutex;
RT_MUTEX statusMutex;

Elevator::Elevator(int lift_num) {
	rt_mutex_create(&directionMutex, NULL);
	rt_mutex_create(&curFloorMutex, NULL);
	rt_mutex_create(&hallCallsMutex, NULL);
	rt_mutex_create(&internalCallsMutex, NULL);
	rt_mutex_create(&destFloorMutex, NULL);
	rt_mutex_create(&statusMutex, NULL);

	this->car_num=lift_num;

	this->setCur_floor(0);
	this->setDest_floor(this->getCur_floor()+1);
	this->setDirection(UP);
	this->setStatus(SPOOLING);

	memset(cur_hall_calls, 0, sizeof(int8_t)*MAX_FLOORS);
	memset(internal_calls, 0, sizeof(int8_t)*MAX_FLOORS);

	internal_calls[getDest_floor()] = 1;
}

Elevator::~Elevator() {
	rt_mutex_delete(&directionMutex);
	rt_mutex_delete(&curFloorMutex);
	rt_mutex_delete(&hallCallsMutex);
	rt_mutex_delete(&internalCallsMutex);
	rt_mutex_delete(&destFloorMutex);
	rt_mutex_delete(&statusMutex);
}

const int8_t Elevator::getCar_num() {
	return car_num;
}

const int8_t Elevator::getCur_floor()
{
	int8_t tempFloor;
	rt_mutex_acquire(&curFloorMutex, TM_INFINITE);
    tempFloor = cur_floor;
    rt_mutex_release(&curFloorMutex);
    return tempFloor;
}

void Elevator::getCur_hall_calls(int8_t tempCalls[MAX_FLOORS] )
{
	rt_mutex_acquire(&curFloorMutex, TM_INFINITE);
	for (int i = 0; i < MAX_FLOORS; i++){
		if (this->cur_hall_calls[i]) {
			rt_printf("Current Hall Call at %d\n\n", i);
		}
		tempCalls[i] = this->cur_hall_calls[i];
	}
	rt_mutex_release(&curFloorMutex);
}

const int8_t Elevator::getDest_floor()
{
	int8_t tempFloor;
	rt_mutex_acquire(&curFloorMutex, TM_INFINITE);
	tempFloor = dest_floor;
	rt_mutex_release(&curFloorMutex);
	return tempFloor;
}

const int8_t Elevator::getDirection()
{
	int8_t tempDir;
	rt_mutex_acquire(&directionMutex, TM_INFINITE);
	tempDir = this->direction;
	rt_mutex_release(&directionMutex);
	return tempDir;
}

void Elevator::getInternal_calls(int8_t tempCalls[MAX_FLOORS])
{
	rt_mutex_acquire(&internalCallsMutex, TM_INFINITE);
	for (int i = 0; i < MAX_FLOORS; i++){
		tempCalls[i] = this->internal_calls[i];
	}
	rt_mutex_release(&internalCallsMutex);
}

const int8_t Elevator::getStatus()
{
	int8_t tempStatus;
	rt_mutex_acquire(&statusMutex, TM_INFINITE);
	tempStatus = this->status;
	rt_mutex_release(&statusMutex);
	return tempStatus;
}

void Elevator::setCur_floor(int8_t floor)
{
    if((floor >= 0) && (floor < MAX_FLOORS)){
    	rt_mutex_acquire(&curFloorMutex, TM_INFINITE);
    	this->cur_floor = floor;
    	rt_mutex_release(&curFloorMutex);
    }

}

void Elevator::add_hall_calls(int8_t new_hall_calls[MAX_FLOORS])
{
	rt_mutex_acquire(&curFloorMutex, TM_INFINITE);
	for (int i =0; i < MAX_FLOORS; i++){

		if(new_hall_calls[i]){
			rt_printf("Added Hall Call %d\n", i);
			cur_hall_calls[i]=1;
		}
	}
	rt_mutex_release(&curFloorMutex);

}

void Elevator::cancel_hall_calls(int8_t cancel_hall_calls[MAX_FLOORS])
{
	rt_mutex_acquire(&curFloorMutex, TM_INFINITE);
	for (int i =0; i < MAX_FLOORS; i++){

		if(cancel_hall_calls[i]){
			this->cur_hall_calls[i]=0;
		}
	}
	rt_mutex_release(&curFloorMutex);
}

void Elevator::setDest_floor(int8_t floor)
{
	if((floor >= 0) && (floor < MAX_FLOORS)){
		rt_mutex_acquire(&destFloorMutex, TM_INFINITE);
		this->dest_floor = floor;
		rt_mutex_release(&destFloorMutex);
	}
}

void Elevator::setDirection(int8_t dir)
{
	if((dir == UP) || (dir == DOWN)){
		rt_mutex_acquire(&directionMutex, TM_INFINITE);
		this->direction = dir;
		rt_mutex_release(&directionMutex);
	}
}

void Elevator::addInternal_call(int8_t floor)
{
	if((floor >= 0) && (floor < MAX_FLOORS)){
		rt_mutex_acquire(&internalCallsMutex, TM_INFINITE);
		this->internal_calls[floor] = 1;
		rt_mutex_release(&internalCallsMutex);
	}
}

void Elevator::setStatus(int8_t iStatus)
{

	if ((iStatus==MOVING) || (iStatus==STOPPED) || (iStatus==SPOOLING)
			|| (iStatus==OPENING) || (iStatus==CLOSING)){

		rt_mutex_acquire(&statusMutex, TM_INFINITE);
		this->status = iStatus;
		rt_mutex_release(&statusMutex);
	}

}

void Elevator::remove_internal_call(int8_t floor){
	rt_mutex_acquire(&internalCallsMutex, TM_INFINITE);
	this->internal_calls[floor] = 0;
	rt_mutex_release(&internalCallsMutex);
}
